/**
 * @file distance.c
 * @author Sevenfite (lin481399413@163.com)
 * @brief 实现距离的监测
 * @version 0.1
 * @date 2024-06-28
 * 
 * @copyright Copyright (c) 2024
 * 
 */

#include "distance.h"
#include <stdint.h>
#include <stdlib.h>
#include "motor.h"
#include "myconfig.h"
struct MyDistanceTypeDef{
    uint32_t id;
    int32_t startDistance;
    struct MyDistanceTypeDef *next;
};
static struct MyDistanceTypeDef *head = NULL;
/**
 * @brief 根据id寻找节点
 * 
 * @param id 
 * @return struct MyDistanceTypeDef* 
 */
struct MyDistanceTypeDef * findMyDistance(uint32_t id){
    struct MyDistanceTypeDef *p = head;
    while(p != NULL){
        if(p->id == id){
            return p;
        }
        p = p->next;
    }
    return NULL;
}
#include "C_code_Log.h"
/**
 * @brief 添加距离节点
 * 
 * @param id 
 * @param startDistance 
 */
static void addMyDistance(uint32_t id, uint32_t startDistance){
    struct MyDistanceTypeDef *p = head;
    struct MyDistanceTypeDef *new = (struct MyDistanceTypeDef *)malloc(sizeof(struct MyDistanceTypeDef));
    if(new == NULL){
        LOG_ERROR("Mydistance malloc failed");
        return;
    }
    new->id = id;
    new->startDistance = startDistance;
    new->next = NULL;
    if(p == NULL){
        head = new;
    }else{
        while(p->next != NULL){
            p = p->next;
        }
        p->next = new;
    }
}
static void deleteMyDistance(uint32_t id){
    struct MyDistanceTypeDef *p = head;
    struct MyDistanceTypeDef *pre = NULL;
    while(p != NULL){
        if(p->id == id){
            if(pre == NULL){
                head = p->next;
            }else{
                pre->next = p->next;
            }
            free(p);
            return;
        }
        pre = p;
        p = p->next;
    }
}
/**
 * @brief 距离函数，不阻塞
 * 
 * @param id 唯一的值，用于区分不同的监测者，建议传入函数名强转的值
 * @param Distance 要监测的距离,单位mm,可正可负
 * @return true 超出距离范围
 * @return false 在距离范围内
 * @note 首次调用时自动根据id添加距离监测任务并返回false，延时完成后返回true并自动删除延时任务
 * @warning 请不要在没有返回true的情况下停止对其的轮询，否则下次进入时会被上次的延时任务干扰
 * @example 
 *          while(1){
 * 			if(MyDistance((uint32_t)main,3000)==true){
 *				LCD_DisplayChar(Line1,1,'1');break;//走3000mm后显示1
 *			    }	
 *          }
 */
bool MyDistance(uint32_t id, int32_t Distance){
    if(Distance == 0)
        return true;
    struct MyDistanceTypeDef *p = findMyDistance(id);
    if(p == NULL){
        addMyDistance(id, gl_motor.encoderDistance);
    }else{
        if(!inRange(gl_motor.encoderDistance,p->startDistance,p->startDistance+Distance*ENCODE_OF_ONE_mm)){
            deleteMyDistance(id);
            return true;
        }
    }
    return false;
}